#include "plane.h"
#include "Fitting/gauss/FittingPlanePCA.h"
#include <Eigen/Core>
#include <Eigen/Sparse>
#include <Eigen/Dense>

namespace  ELEMENT {
    Plane::Plane():Element()
    {
        type = ElementType::Plane;
    }

    bool Plane::fit(const std::vector<Point> &points)
    {
        double err = Gauss::FittingPCA(points, plane);
        if(err<0) {
            return false;
        }

        vertices.resize(4);
        // 找到4个点
        double lmax=0;
        for(auto &p: points) {
            auto k=(p-plane.BasePoint).cross(plane.Orient);
            double l = k.norm();
            if(l>lmax){
                lmax=l;
                vertices[0]=plane.BasePoint+k;
            }
        }
        auto ax = Eigen::AngleAxisd(M_PI/2, plane.Orient).toRotationMatrix();
        for(int i=1;i<4;++i) {
            vertices[i]=ax*(vertices[i-1]-plane.BasePoint)+plane.BasePoint;
        }

        index.resize(6);
        for(int i=0;i<3;++i) index[i]=i;
        index[3]=0;
        index[4]=2;
        index[5]=3;
        return true;
    }

    void Plane::Draw(QOpenGLShaderProgram &shader)
    {
        shader.bind();
        shader.setUniformValue("userColor",QVector4D(0.0f, 0.5f, 0.0f, 1.0f));
        QOpenGLVertexArrayObject::Binder bind(&VAO);
        glDrawElements(GL_TRIANGLES, index.size(), GL_UNSIGNED_INT, 0);
    }

    QString Plane::Info()
    {
        QString info("类型：平面\n中心点：%2 %3 %4\n法向：%5 %6 %7");
        return info
                .arg(plane.BasePoint.x()).arg(plane.BasePoint.y()).arg(plane.BasePoint.z())
                .arg(plane.Orient.x()).arg(plane.Orient.y()).arg(plane.Orient.z());
    }

}
